global initTheta;
initTheta = readmatrix('InitTheta.xlsx');
initTheta = deg2rad(initTheta);
% 初始化至(-pi, pi]
for row = 1 : 10
    for col = 1 : 12
        if initTheta(row, col) <= -pi
            initTheta(row, col) = initTheta(row, col) + 2 * pi;
        end
        if initTheta(row, col) > pi
            initTheta(row, col) = initTheta(row, col) - 2 * pi;
        end
    end
end

global ThetaNow;
ThetaNow(1, :) = initTheta(4, :);
ThetaNow(2, :) = initTheta(8, :);
ThetaNow(3, :) = initTheta(10, :);

% global Data
Data = readmatrix('AngelData.xlsx');
Data = deg2rad(Data);
% r0 = [255.4006667; 266.6986667; 31.99766667; 210.07425; 172.4989167; 221.92825; 30.27241667;...
%     61.72641667; 316.10425; 258.2720833; 223.3456667; 58.24991667];
r0 = [255.7829517; 266.1321547; 32.06196806; 210.2738735; 171.6391334; 221.7470959;...
    30.23200964; 61.64972268; 315.7809141; 257.400443; 223.0561673; 58.20227293];
epsilon = 0.001;
delta = 0.03;       % 学习率
N = 100000;          % 迭代次数

% debug
%for i = 1 : 12
    Paint(r0, initTheta(:,1), 1, "#B500B0");
    Paint(r0, initTheta(:,5), 5, "#B500B0");
    Paint(r0, initTheta(:,9), 9, "#B500B0");
%end

try
    plx = [0 02 03 06 07 09 11 14 16 18 20 23];
    figure(1);
    plot(plx, Data(3,:), 'black');hold on;
    plot(plx, initTheta(4, :), 'Color',"#0000B0");hold on;
    title('$\theta_{7}$', 'Interpreter','latex');
    xlabel('t');
    ylabel('$\theta$', 'Interpreter','latex');
    
    figure(2);
    plot(plx, Data(2,:), 'black');hold on;
    plot(plx, initTheta(8, :), 'Color',"#B500B5");hold on;
    title('$\theta_{12}$', 'Interpreter','latex');
    xlabel('t');
    ylabel('$\theta$', 'Interpreter','latex');

    figure(3);
    plot(plx, Data(1,:), 'black');hold on;
    plot(plx, initTheta(10, :), 'Color',"#B30000");hold on;
    title('$\theta_{14}$', 'Interpreter','latex');
    xlabel('t');
    ylabel('$\theta$', 'Interpreter','latex');

    k = lamdaH(r0, Data);
    i = 0;
    r = r0;
    debugK = zeros([12 100]);
    debugNormK = zeros([1 100]);
    while i < N && norm(k) >= epsilon
        i = i + 1;
        % debug
        % disp(k);
        debugK(:, round(mod(i-1, 100)) + 1) = k;
        debugNormK(1, round(mod(i-1, 100)) + 1) = norm(k);
        % disp(norm(k));
        
        if mod(i, 5000) == 0
            % debug
            %for i = 1 : 12
                Paint(r0, initTheta(:,1), 1, "#0000FF");
                Paint(r0, initTheta(:,5), 5, "#0000FF");
                Paint(r0, initTheta(:,9), 9, "#0000FF");
            %end

            plx = [0 02 03 06 07 09 11 14 16 18 20 23];
            figure(1);
            plot(plx, ThetaNow(1, :), 'blue');
            
            figure(2);
            plot(plx, ThetaNow(2, :), 'magenta');

            figure(3);
            plot(plx, ThetaNow(3, :), 'red');
            % pause;
            disp(norm(k));
        end
        
        k = lamdaH(r, Data);
        r = r - delta * k;

        % if delta > 0.01
        %     delta = delta - 0.09/2000;
        % end
        if k(1)<=0 && k(2)<=0 && k(3)>=0 && k(4)<= 0 && k(5)>=0 &&...
                k(6)<=0 && k(7)>=0 && k(8)>=0 && k(9)>=0 && k(10)<=0 && k(12)<=0
            disp(norm(k));
            disp(r);
            disp(i);
        end
    end
catch ME
    disp('main: i = ');
    disp(i);
    disp('norm(k) = ');
    disp(norm(k));
    rethrow(ME);
end
disp('N=');
disp(N);
disp('r=');
disp(r);


function res = lamdaH(r, data)
    data7 = data(3, :);  % size of data: 3 12
    data12 = data(2,:);
    data14 = data(1, :);

    res = zeros([12 1]);
    try
        for i = 1:12
            theta = g(r, i);
            % debug
            % Paint(r, theta, i);
            theta7 = theta(4);
            theta12 = theta(8);
            theta14 = theta(10);
            Dg = D_g(r, theta);
            res = res + ((theta7 - data7(i)) * (Dg(1, :).') + (theta12 - data12(i)) * (Dg(2, :).') + (theta14 - data14(i)) * (Dg(3,:).')) / ...
                sqrt((theta7 - data7(i)) * (theta7 - data7(i)) + (theta12 - data12(i)) * (theta12 - data12(i)) + (theta14 - data14(i)) * (theta14 - data14(i)));
        end
    catch ME
        causeException = MException('MATLAB:lamdaH',strcat('lamdaH: i = ',num2str(i)));
        ME = addCause(ME,causeException);
        rethrow(ME);
    end
end

function res = D_g(r, theta)
    % res = (D_g_7;D_g_12;D_g_14)
    % 常量
    r6 = 41.575;
    
    alpha57 = 0;
    alpha1112 = 0;
    alpha1415 = -deg2rad(28.592);

    % 形参
    r3 = r(1);
    r4 = r(2);
    r5 = r(3);
    r7 = r(4);
    r8 = r(5);
    r9 = r(6);
    r10 = r(7);
    r11 = r(8);
    r12 = r(9);
    r13 = r(10);
    % r14 = r(11);  % r14不参与后续方程计算
    r15 = r(12);

    % theta1 = phi0 + (pi / 12) * (t - 9); % 24帧为一周期，初值对应第9帧
    % theta2 = theta1 + alpha12;
    theta3 = theta(1);
    theta4 = theta(2);
    theta6 = theta(3);
    theta7 = theta(4);
    theta8 = theta(5);
    theta9 = theta(6);
    theta10 = theta(7);
    theta12 = theta(8);
    theta13 = theta(9);
    theta14 = theta(10);

    % in file symInfo.mlx
    D_F_theta = [-r3*sin(theta3), 0, 0, -r5*sin(alpha57 - theta7), 0, 0, 0, 0, 0, 0;
                 r3*cos(theta3), 0, 0, -r5*cos(alpha57 - theta7), 0, 0, 0, 0, 0, 0;
                 0, -r4*sin(theta4), r6*sin(theta6), 0, 0, 0, 0, 0, 0, 0;
                 0, r4*cos(theta4), -r6*cos(theta6), 0, 0, 0, 0, 0, 0, 0;
                 0, 0, r6*sin(theta6), -r7*sin(theta7), r8*sin(theta8), 0, -r10*sin(theta10), 0, 0, 0;
                 0, 0, -r6*cos(theta6), r7*cos(theta7), -r8*cos(theta8), 0, r10*cos(theta10), 0, 0, 0;
                 0, 0, r6*sin(theta6), -r7*sin(theta7), 0, r9*sin(theta9), 0, r11*sin(alpha1112 - theta12), 0, 0;
                 0, 0, -r6*cos(theta6), r7*cos(theta7), 0, -r9*cos(theta9), 0, r11*cos(alpha1112 - theta12), 0, 0;
                 0, 0, 0, 0, 0, 0, r10*sin(theta10), -r12*sin(theta12), r13*sin(theta13), r15*sin(alpha1415 + theta14);
                 0, 0, 0, 0, 0, 0, -r10*cos(theta10), r12*cos(theta12), -r13*cos(theta13), -r15*cos(alpha1415 + theta14)];
    D_F_r = [cos(theta3), 0, -cos(alpha57 - theta7), 0, 0, 0, 0, 0, 0, 0, 0, 0;
             sin(theta3), 0, sin(alpha57 - theta7), 0, 0, 0, 0, 0, 0, 0, 0, 0;
             0, cos(theta4), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
             0, sin(theta4), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
             0, 0, 0, cos(theta7), -cos(theta8), 0, cos(theta10), 0, 0, 0, 0, 0;
             0, 0, 0, sin(theta7), -sin(theta8), 0, sin(theta10), 0, 0, 0, 0, 0;
             0, 0, 0, cos(theta7), 0, -cos(theta9), 0, cos(alpha1112 - theta12), 0, 0, 0, 0;
             0, 0, 0, sin(theta7), 0, -sin(theta9), 0, -sin(alpha1112 - theta12), 0, 0, 0, 0;
             0, 0, 0, 0, 0, 0, -cos(theta10), 0, cos(theta12), -cos(theta13), 0, -cos(alpha1415 + theta14);
             0, 0, 0, 0, 0, 0, -sin(theta10), 0, sin(theta12), -sin(theta13), 0, -sin(alpha1415 + theta14)];
    D_g_r = - inv(D_F_theta) * D_F_r;

    res = zeros([3 12]);
    res(1, :) = D_g_r(4, :);    % theta7
    res(2, :) = D_g_r(8, :);    % theta12
    res(3, :) = D_g_r(10, :);   % theta14
end

function res = g(r, t)
    % picture9 对应的theta0-15
    % [-155.712, -67.068, -152.850, -152.179, -148.132, -59.490, -96.953, -57.298, -58.751, -61.647, -157.671,...
    %   -102.430, -102.430, -105.625, -15.716, -59.300];
    % 对应的r1-15
    % [2.572, 2.213, 37.320, 38.830, 4.678, 6.058, 30.608, 25.043, 32.354, 4.411, 8.995, 46.074, 37.556, 32.545, 8.492]
    % theta0 = [-152.179, -148.132, -96.953, -57.298, -58.751, -61.647, -157.671, -102.430, -105.625, -15.716];
    global initTheta;
    opts = optimoptions(@fsolve,'Algorithm', 'levenberg-marquardt', 'display', 'off');
    % opts = optimoptions(@fsolve, 'display', 'off');
    [res,~,exitflag,output] = fsolve(@(theta)Fun(theta, r, t), initTheta(:, t), opts);
    % global Data;
    % [res,~,exitflag,output] = fsolve(@(theta)Fun(theta, r, t), Data(:, t), opts);
    if exitflag <= 0
        disp(output.message);
        ME = MException('Matlab:g:fsolve','Error in fsolve. On line 127');
        throw(ME);
    else
        % 归一化至(-pi, pi]
        for row = 1 : 10
            if res(row) <= -pi
                res(row) = res(row) + 2 * pi;
            end
            if res(row) > pi
                res(row) = res(row) - 2 * pi;
            end
        end
        initTheta(:, t) = res;
        global ThetaNow
        ThetaNow(1, t) = res(4);
        ThetaNow(2, t) = res(8);
        ThetaNow(3, t) = res(10);
    end
end

function res = Fun(theta, r, t)
    % 常量
    r0 = 258.22975;
    theta0 = deg2rad(25.76408333);
    r1 = 1.22679196;
    r2 = 15.89042;
    r6 = 41.575;
    
    alpha12 = -deg2rad(85.379);
    alpha57 = 0;
    alpha1112 = 0;
    alpha1415 = -deg2rad(28.592);
    phi0 = deg2rad(16.808);    % 对应picture9
    lis = [0 02 03 06 07 09 11 14 16 18 20 23];
    
    % 形参
    % t为对应帧序号，非真实时间
    r3 = r(1);
    r4 = r(2);
    r5 = r(3);
    r7 = r(4);
    r8 = r(5);
    r9 = r(6);
    r10 = r(7);
    r11 = r(8);
    r12 = r(9);
    r13 = r(10);
    % r14 = r(11);  % r14不参与后续方程计算
    r15 = r(12);
    
    theta1 = phi0 + (pi / 12) * lis(t); % 24帧为一周期，初值对应第9帧
    theta2 = theta1 + alpha12;
    theta3 = theta(1);
    theta4 = theta(2);
    theta6 = theta(3);
    theta7 = theta(4);
    theta8 = theta(5);
    theta9 = theta(6);
    theta10 = theta(7);
    theta12 = theta(8);
    theta13 = theta(9);
    theta14 = theta(10);
    
    % 约束
    theta5 = theta7 - alpha57;
    theta11 = theta12 - alpha1112;
    theta15 = theta14 + alpha1415;
    
    % F
    res = zeros(10);
    
    res(1) = r0*cos(theta0) + r1*cos(theta1) + r3*cos(theta3) - r5*cos(theta5);
    res(2) = r0*sin(theta0) + r1*sin(theta1) + r3*sin(theta3) - r5*sin(theta5);
    res(3) = r0*cos(theta0) + r2*cos(theta2) + r4*cos(theta4) - r6*cos(theta6);
    res(4) = r0*sin(theta0) + r2*sin(theta2) + r4*sin(theta4) - r6*sin(theta6);
    res(5) = r7*cos(theta7) + r10*cos(theta10) - r8*cos(theta8) - r6*cos(theta6);
    res(6) = r7*sin(theta7) + r10*sin(theta10) - r8*sin(theta8) - r6*sin(theta6);
    res(7) = r7*cos(theta7) + r11*cos(theta11) - r9*cos(theta9) - r6*cos(theta6);
    res(8) = r7*sin(theta7) + r11*sin(theta11) - r9*sin(theta9) - r6*sin(theta6);
    res(9) = r12*cos(theta12) - r15*cos(theta15) - r13*cos(theta13) - r10*cos(theta10);
    res(10) = r12*sin(theta12) - r15*sin(theta15) - r13*sin(theta13) - r10*sin(theta10);
end

function res = Paint(r, theta, t, col)
    % 常量
    r0 = 258.22975;
    theta0 = deg2rad(25.76408333);
    r1 = 17.62791403;
    r2 = 15.16740815;
    r6 = 41.5201801;
    
    alpha12 = deg2rad(-84.24783333);
    alpha57 = 0;
    alpha1112 = 0;
    alpha1415 = deg2rad(-43.7905);
    phi0 = deg2rad(16.808);    % 对应picture9
    lis = [0 02 03 06 07 09 11 14 16 18 20 23];
    
    % 形参
    % t为对应帧序号，非真实时间
    r3 = r(1);
    r4 = r(2);
    r5 = r(3);
    r7 = r(4);
    r8 = r(5);
    r9 = r(6);
    r10 = r(7);
    r11 = r(8);
    r12 = r(9);
    r13 = r(10);
    r14 = r(11);  % r14不参与后续方程计算
    r15 = r(12);
    
    theta1 = phi0 + (pi / 12) * lis(t); % 24帧为一周期，初值对应第9帧
    theta2 = theta1 + alpha12;
    theta3 = theta(1);
    theta4 = theta(2);
    theta6 = theta(3);
    theta7 = theta(4);
    theta8 = theta(5);
    theta9 = theta(6);
    theta10 = theta(7);
    theta12 = theta(8);
    theta13 = theta(9);
    theta14 = theta(10);
    
    % 约束
    theta5 = theta7 - alpha57;
    theta11 = theta12 - alpha1112;
    theta15 = theta14 + alpha1415;

    % point
    Ax = 0; Ay = 0;
    Bx = -r0*cos(theta0); By = -r0*sin(theta0);
    Fx = r1*cos(theta1); Fy = r1*sin(theta1);
    Ex = r2*cos(theta2); Ey = r2*sin(theta2);
    Gx = Fx + r3*cos(theta3); Gy = Fy + r3*sin(theta3);
    Ix = Ex + r4*cos(theta4); Iy = Ey + r4*sin(theta4);
    Dx = Bx + r7*cos(theta7); Dy = By + r7*sin(theta7);
    Lx = Ix + r8*cos(theta8); Ly = Iy + r8*sin(theta8);
    Hx = Ix + r9*cos(theta9); Hy = Iy + r9*sin(theta9);
    Cx = Dx + r12*cos(theta12); Cy = Dy + r12*sin(theta12);
    Kx = Lx + r13*cos(theta13); Ky = Ly + r13*sin(theta13);
    Jx = Cx + r14*cos(theta14); Jy = Cy + r14*sin(theta14);

    res = figure(40 + t);
    %clf(40 + t);
    quiver(Ax,Ay,Bx-Ax,By-Ay,'Color',col);hold on;
    quiver(Ax,Ay,Fx-Ax,Fy-Ay,'Color',col);hold on;
    quiver(Ax,Ay,Ex-Ax,Ey-Ay,'Color',col);hold on;
    quiver(Fx,Fy,Gx-Fx,Gy-Fy,'Color',col);hold on;
    quiver(Ex,Ey,Ix-Ex,Iy-Ey,'Color',col);hold on;
    quiver(Bx,By,r5*cos(theta5),r5*sin(theta5),'Color',col);hold on;
    quiver(Bx,By,r6*cos(theta6),r6*sin(theta6),'Color',col);hold on;
    quiver(Bx,By,Dx-Bx,Dy-By,'Color',col);hold on;
    quiver(Ix,Iy,Lx-Ix,Ly-Iy,'Color',col);hold on;
    quiver(Ix,Iy,Hx-Ix,Hy-Iy,'Color',col);hold on;
    quiver(Dx,Dy,r10*cos(theta10),r10*sin(theta10),'Color',col);hold on;
    quiver(Dx,Dy,r11*cos(theta11),r11*sin(theta11),'Color',col);hold on;
    quiver(Dx,Dy,Cx-Dx,Cy-Dy,'Color',col);hold on;
    quiver(Lx,Ly,Kx-Lx,Ky-Ly,'Color',col);hold on;
    quiver(Cx,Cy,Jx-Cx,Jy-Cy,'Color',col);hold on;
    quiver(Kx,Ky,r15*cos(theta15),r15*sin(theta15),'Color',col);hold on;
    title('Mechanism');

end